#include <string.h>
#include <errno.h>
#include <poll.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>

#include "uart_send.h"


static char * cmd_to_string(uint8_t);

static inline int is_printable(char c) {
    return (c > 0x2F && c < 0x3A) || \
        (c > 0x40 && c < 0x5B) || \
        (c > 0x60 && c < 0x7B) ? 1 : 0;
}

/*
 * cmd  The command, listed in fmux.h
 * data Payload
 * len  Size of the payload in bytes
 */
void uart_send(uint8_t cmd, uint8_t const * data, uint8_t len) {
    struct comm_unit * msg = (struct comm_unit *) malloc(sizeof(struct comm_unit)+len);
    struct timeval time;

    if (msg == NULL) {
        perror("uart_send");
        return;
    }

    msg->size = len;
    msg->cmd = cmd;
    for (int i = 0; i < len; i++) {
        msg->data[i] = data[i];
    }

    ssize_t r = write(SERIAL_FD, msg, sizeof(struct comm_unit)+msg->size);

    if (r == -1) {
        perror("uart_send");
    }

    if (cmd == RADIOSETPSDU) {
        DBG("\n> %hhu, " GREEN "%s\n" CLR, len, cmd_to_string(cmd));
        for (int i = 0; i < len; i++) {
            if (is_printable(data[i])) {
                DBG("%x(" YELLOW "%c" CLR "), ", data[i], data[i]);
            } else {
                DBG("%x( ), ", data[i]);
            }
        }
        if (len) {
            DBG("\n");
        }
        gettimeofday(&time, NULL);
        DBG("[%ld.%06ld]> sent: " CYAN "%zd" CLR " bytes\n", time.tv_sec, time.tv_usec, r);
    }

    free(msg);
}

uint8_t uart_recv(uint8_t * data, uint8_t len) {
    //TODO:
    uint8_t header[2];
    ssize_t r;
    struct timeval time;

    const int flags = POLLIN | POLLRDNORM | POLLERR | POLLNVAL | POLLHUP;
    struct pollfd pollfd = { SERIAL_FD, flags, 0 };

    if (! (poll(&pollfd, 1, 0) > 0 && (pollfd.revents & flags) != 0) )
    {
        return 0;
    }

    r= read(SERIAL_FD, header, 2);

    if (r == -1) {
        perror("uart_recv");
    }
    /*if (len != header[0]) {
        DBG("receivng length does not match expected length! 0x%x : 0x%x\n", len, header[0]);
    }*/
    if (header[0] > len) {
        printf("Recv Buffer too small for incoming message %d > %d\n", header[0], len);
        exit(1);
    }

    while (header[1] == 0xAA) {
        char buffer[256];

        r = read(SERIAL_FD, buffer, header[0]);
        buffer[header[0]+3] = 0;
        gettimeofday(&time, NULL);
        DBG(MAG "[%06ld]DBG %s\n" CLR, time.tv_usec, buffer);

        const int flags = POLLIN | POLLRDNORM | POLLERR | POLLNVAL | POLLHUP;
        struct pollfd pollfd = { SERIAL_FD, flags, 0 };

        if (! (poll(&pollfd, 1, 0) > 0 && (pollfd.revents & flags) != 0) )
        {
            return 0;
        }

        r= read(SERIAL_FD, header, 2);
    }

    r = read(SERIAL_FD, data, header[0]);

    if (r == -1) {
        perror("uart_recv");
    }

    if (header[0] == 0) {
        DBG(MAG "0" CLR);
    } else {
        DBG("\n> %zd, " MAG "response\n" CLR, r);
        for (int i = 0; i < r; i++) {
            if (is_printable(data[i])) {
                DBG("%x(" YELLOW "%c" CLR "), ", data[i], data[i]);
            } else {
                DBG("%x( ), ", data[i]);
            }
        }
        if (r) {
            DBG("\n");
        }
        gettimeofday(&time, NULL);
        DBG("[%ld.%06ld]> recvd: " CYAN "%zd" CLR " bytes\n", time.tv_sec, time.tv_usec, r);
    }

    return (uint8_t) r;
}

void sendInit() {
    return;
}

static char * cmd_to_string(uint8_t cmd) {
    switch (cmd) {
        case 0: return "OTPLATRADIOGETIEEEEUI64";
        case 1: return "OTPLATRADIOSETPANID";
        case 2: return "OTPLATRADIOSETEXTENDEDADDRESS";
        case 3: return "OTPLATRADIOSETSHORTADDRESS";
        case 4: return "OTPLATRADIOGETSTATE";
        case 5: return "OTPLATRADIOENABLE";
        case 6: return "OTPLATRADIODISABLE";
        case 7: return "OTPLATRADIOISENABLED";
        case 8: return "OTPLATRADIOSLEEP";
        case 9: return "OTPLATRADIORECEIVE";
        case 10: return "OTPLATRADIOENABLESRCMATCH";
        case 11: return "OTPLATRADIOADDSRCMATCHSHORTENTRY";
        case 12: return "OTPLATRADIOADDSRCMATCHEXTENTRY";
        case 13: return "OTPLATRADIOCLEARSRCMATCHSHORTENTRY";
        case 14: return "OTPLATRADIOCLEARSRCMATCHEXTENTRY";
        case 15: return "OTPLATRADIOCLEARSRCMATCHSHORTENTRIES";
        case 16: return "OTPLATRADIOCLEARSRCMATCHEXTENTRIES";
        case 17: return "OTPLATRADIOGETTRANSMITBUFFER";
        case 18: return "OTPLATRADIOTRANSMIT";
        case 19: return "OTPLATRADIOGETRSSI";
        case 20: return "OTPLATRADIOGETCAPS";
        case 21: return "OTPLATRADIOGETTRANSMITPOWER";
        case 22: return "OTPLATRADIOSETTRANSMITPOWER";
        case 23: return "OTPLATRADIOGETPROMISCUOUS";
        case 24: return "OTPLATRADIOSETPROMISCUOUS";
        case 25: return "OTPLATRADIOENERGYSCAN";
        case 26: return "OTPLATRADIOGETRECEIVESENSITIVITY";
        case 27: return "RADIOPOLLFRAME";
        case 28: return "RADIOSETCHANNEL";
        case 29: return "RADIOSETPSDULEN";
        case 30: return "RADIOSETPSDU";

        case 128: return "OTPLATRADIORECEIVEDONE";
        case 129: return "OTPLATRADIOTXSTARTED";
        case 130: return "OTPLATRADIOTXDONE";
        case 131: return "OTPLATDIAGRADIOTRANSMITDONE";
        case 132: return "OTPLATDIAGRADIORECEIVEDONE";
        case 133: return "OTPLATRADIOENERGYSCANDONE";
        default: return "error";
    }
}
